#include "stdafx.h"
#include "CollisionDetection.h"
#include "Robot.h"

using namespace RobotCollisionDetection;

// inicjalizacja obiektu statycznego
std::vector<CollisionDetection *> CollisionDetection::CollisionDetectionModels;

CollisionDetection::CollisionDetection()
{
	CollisionDetection::CollisionDetectionModels.push_back(this);
}

void RapidPrepareTri(double toRapid[3][3], Triangle triangle)
{    
    toRapid[0][0] = triangle.vertex0[0].x;
    toRapid[0][1] = triangle.vertex0[0].y;
    toRapid[0][2] = triangle.vertex0[0].z;

    toRapid[1][0] = triangle.vertex0[1].x;
    toRapid[1][1] = triangle.vertex0[1].y;
    toRapid[1][2] = triangle.vertex0[1].z;

    toRapid[2][0] = triangle.vertex0[2].x;
    toRapid[2][1] = triangle.vertex0[2].y;
    toRapid[2][2] = triangle.vertex0[2].z;

}

void CollisionDetection::InitializeCollisionDetectionModel()
{
	for (unsigned int module=0; module<this->modules.size(); module++)
	{
		this->modules[module].rapidModel.BeginModel();

		unsigned int i;
		for (i = 0; i < this->modules[module].triangles.size(); i++)
		{
			double toRapid[3][3];
			RapidPrepareTri(toRapid,this->modules[module].triangles[i]);
			this->modules[module].rapidModel.AddTri(toRapid[0], toRapid[1], toRapid[2], i);					
		}

		this->modules[module].rapidModel.EndModel();
		

		if (this->isRobot)
		{			
			// indeksy wierzcholkow :
			//int vect[] = {0,3,7,4,1,2,6,5,3,2,6,7,0,1,5,4,5,6,7,4,1,2,3,0};

			this->modules[module].rapidModelForCubes.BeginModel();

			int tIndex[] = {0,1,5,
							0,4,5,
							1,2,6,
							1,6,5,
							2,3,6,
							3,6,7,
							3,0,7,
							0,7,4,
							4,5,7,
							5,6,7,
							1,2,3,
							0,1,3
						};
			for (int i = 0; i < 12; i++) {
				//this->modules[module].cModule.vertexes0
				Vertex v = this->modules[module].cModule.vertexes0[tIndex[i*3]];
				double v1[] = {v.x, v.y, v.z};
				v = this->modules[module].cModule.vertexes0[tIndex[i*3+1]];
				double v2[] = {v.x, v.y, v.z};
				v = this->modules[module].cModule.vertexes0[tIndex[i*3+2]];
				double v3[] = {v.x, v.y, v.z};
				this->modules[module].rapidModelForCubes.AddTri(v1,v2,v3,i);
			}
			
			this->modules[module].rapidModelForCubes.EndModel();
		}
	}
}


int CollisionDetection::FindFirstCollisionInAllObjects()
{
	static int collidedModels[2]={-1, -1};
    unsigned int i,j;

	// zerowanie
	for (i=0; i<CollisionDetectionModels.size(); i++)
		CollisionDetectionModels[i]->collidedModule=-1;

	// sprawdzamy kolizje modelu [n] z modelami [n+1,...]
    for (i=0; i<CollisionDetectionModels.size(); i++)
    {
		
		if (CollisionDetectionModels[i]->isRobot)
		{
			// kolizje wewnatrz robota
			if (FindFirstCollisionInModule(CollisionDetectionModels[i]))
			{
				if (!(collidedModels[0]==i && collidedModels[1]==-1))
				{
					collidedModels[0]=i;
					collidedModels[1]=-1;
					Logger::InsertCollisionMessage("Kolizja w obiekcie nr " + i);
				}

				return 1;
			}
		}

		for (j=i+1; j<CollisionDetectionModels.size(); j++)
		{
			if (CollisionDetectionModels[i]->isRobot || CollisionDetectionModels[j]->isRobot)
			{
				// kolizje z otoczeniem
				if (FindFirstCollisionBetweenModules(CollisionDetectionModels[i],CollisionDetectionModels[j]))
				{
					if (!(collidedModels[0]==i && collidedModels[1]==j))
					{
						collidedModels[0]=i;
						collidedModels[1]=j;
						Logger::InsertCollisionMessage("Kolizja pomiedzy obiektami " + i + " i " + j);
					}
					return 1;
				}
			}
		}

    }

	// wyjscie ze stanu kolizji
	if (collidedModels[0]>=0)
	{
		collidedModels[0]=-1;
		collidedModels[1]=-1;
		Logger::InsertCollisionMessage("Wyjscie ze stanu kolizji");
	}
    return 0;

}


int CollisionDetection::FindFirstCollisionBetweenModules(CollisionDetection * model0, CollisionDetection * model1)
{
    unsigned int i,j;
	for (i=0;i<model0->modules.size();i++)
    {
        for (j=0;j<model1->modules.size();j++)
        {		

			RapidModelType model0Type;
			RapidModelType model1Type;

			if (model0->isRobot)
				model0Type = RapidCubes;
			else
				model0Type = RapidTriangles;

			if (model1->isRobot)
				model1Type = RapidCubes;
			else
				model1Type = RapidTriangles;

			if (CollisionDetectionInModules(&(model0->modules[i]), model0Type, &(model1->modules[j]), model1Type))
            {
                model0->collidedModule=i;
                model1->collidedModule=j;
                return 1;
            }
        }
    }

    return 0;
}


int CollisionDetection::FindFirstCollisionInModule(CollisionDetection * model)
{
	if (model->modules.size()>1)
	{
		unsigned int i,j;
		for (i=0;i<model->modules.size()-2;i++)
		{
			for (j=i+2;j<model->modules.size();j++)
			{
				if (CollisionDetectionInModules(&(model->modules[i]), RapidTriangles, &(model->modules[j]), RapidTriangles))
				{
					model->collidedModule=j;
					return 1;
				}
			}
		}
	}
    return 0;
}


int CollisionDetection::CollisionDetectionInModules(RapidModule * model1, RapidModelType model1Type, RapidModule * model2, RapidModelType model2Type)
{
 
	RAPID_model *rapid1;
	RAPID_model *rapid2;

	if (model1Type == RapidCubes)
		rapid1 = &(model1->rapidModelForCubes);
	else
		rapid1 = &(model1->rapidModel);

	if (model2Type == RapidCubes)
		rapid2 = &(model2->rapidModelForCubes);
	else
		rapid2 = &(model2->rapidModel);	

	// now we can perform a collision query:
	RAPID_Collide(model1->Rmat, model1->Tmat, rapid1, model2->Rmat, model2->Tmat, rapid2, RAPID_FIRST_CONTACT);

    //printf("RAPID: %d/%d\n", RAPID_num_box_tests, RAPID_num_contacts);
    //fflush(stdout);

    if (RAPID_num_contacts)
        return 1;
    else
        return 0;
}


int CollisionDetection::RemoveModel(unsigned int i)
{
	// find number of TcpConnection in ConnectionArray
	int tcpConnectionIndex = -1;
	if (CollisionDetectionModels[i]->isRobot)
	{
		for (unsigned int k=0; k<=i; k++)
		{
			if (CollisionDetectionModels[k]->isRobot)
				tcpConnectionIndex++;
		}
	}

	
	if (CollisionDetectionModels.size()>0)
	{	
		Logger::InsertInfoMessage("usuwanie modulu");
				
		delete CollisionDetectionModels[i];
		CollisionDetectionModels.erase(CollisionDetectionModels.begin()+i);
		return tcpConnectionIndex;
	}
	else
		return -1;
	
}
